/*
 *  BlobTracker.cpp
 *  portVideoBlobDetection
 *
 *  Created by Pawe? So?yga on 7/27/07.
 *  Copyright 2007 __MyCompanyName__. All rights reserved.
 *
 */

#include "BlobTracker.h"
#include <math.h>

BlobTracker::BlobTracker()
{
	threshold = 30;
	lastId = 0;
}

void BlobTracker::setThreshold(float threshold)
{
	this->threshold = threshold;
}

bool BlobTracker::addListener(ITouchListener *listener, std::string listenerName)
{
	listenersMap[listenerName] = listener;
	return true;
}

bool BlobTracker::removeListener(ITouchListener *listener)
{
	for (std::map<std::string, ITouchListener *>::iterator it = listenersMap.begin(); it!=listenersMap.end(); ++it)
	{
		if( (*it).second == listener)
			listenersMap.erase( (*it).first );
	}
	return true;
}

bool BlobTracker::removeListener(std::string listenerName)
{
	listenersMap.erase(listenerName);
	return true;
}

void BlobTracker::convertBlobsToTouchData()
{
	currentBlobs.clear();
	for (int n=0 ; n< bd->getBlobNb() ; n++)
	{
		b = bd->getBlob(n);
		if (b.isOk())
		{
				BlobExt be;
				be.td.ID = b.id;
				be.td.X = b.x;
				be.td.Y = b.y;
				be.td.width = b.w;
				be.td.height = b.h;
				currentBlobs.push_back(be);
		}
	}
}

void BlobTracker::track()
{
	convertBlobsToTouchData();
	currentBlobsIds.clear();
	
	if( previousBlobs.size() == 0)
	{
		lastId = 0;
		int x = 0;
		for (std::vector<BlobExt>::iterator it = currentBlobs.begin(); it!=currentBlobs.end(); ++it) 
		{
			doDownEvent( (*it).td);
			currentBlobsIds[ (*it).td.ID] = true;
			x++;
			if( (*it).td.ID > lastId)
				lastId = (*it).td.ID;
		}
		x = x - x;
	}
	else
	{
		calculateAllDistances();
		sortBlobsClosestByDistance();
		sortBlobsByClosest();
		
		for (std::vector<BlobExt>::iterator it = currentBlobs.begin(); it!=currentBlobs.end(); ++it) 
		{
			int i = 0;
			bool notFound = false;
			
			while( currentBlobsIds.find((*it).closest[i].first) != currentBlobsIds.end() )
			{
				i++;
				if( i >= (int) (*it).closest.size() )
				{
					notFound = true;
					break;
				}
			}
			
			// end of closest vector nothing found
			if( notFound )
			{
				(*it).td.ID = getNextFreeId();
				currentBlobsIds[ (*it).td.ID] = true;
				doDownEvent((*it).td);
			}
			else // found something
			{
				if( threshold > (*it).closest[i].second ) 
				{
					(*it).td.ID = (*it).closest[i].first;
					currentBlobsIds[ (*it).td.ID] = true;
					doUpdateEvent((*it).td);
				}
				else 
				{
					(*it).td.ID = getNextFreeId();
					currentBlobsIds[ (*it).td.ID] = true;
					doDownEvent((*it).td);
				}
			}
		}
			
			
		//run upEvent
		for (std::vector<BlobExt>::iterator it = previousBlobs.begin(); it!=previousBlobs.end(); ++it) 
		{
			if (currentBlobsIds.find((*it).td.ID) == currentBlobsIds.end() )
				doUpEvent((*it).td);
		}

	}
		
	previousBlobsIds = currentBlobsIds;
	previousBlobs = currentBlobs;
}

int BlobTracker::getNextFreeId()
{
	lastId++;
	return lastId;
}

void BlobTracker::sortBlobsByClosest()
{
	std::sort(currentBlobs.begin(),currentBlobs.end(),ClosestDistanceSort());
}

float BlobTracker::getDistance(TouchData b1,TouchData b2)
{
	return sqrt((b1.X - b2.X)*(b1.X - b2.X) + (b1.Y - b2.Y)*(b1.Y - b2.Y));
}

void BlobTracker::sortBlobsClosestByDistance()
{
	for (std::vector<BlobExt>::iterator it = currentBlobs.begin(); it!=currentBlobs.end(); ++it) 
	{
		std::sort((*it).closest.begin(),(*it).closest.end(),AscendingDistanceSort());
	}
}

void BlobTracker::calculateAllDistances()
{
	for (std::vector<BlobExt>::iterator currIt = currentBlobs.begin(); currIt!=currentBlobs.end(); ++currIt) 
	{
		(*currIt).closest.clear();
		for( std::vector<BlobExt>::iterator prevIt = previousBlobs.begin(); prevIt!= previousBlobs.end(); ++prevIt)
		{
			(*currIt).closest.push_back(std::pair<int,float>((*prevIt).td.ID, getDistance((*currIt).td,(*prevIt).td) ) );
		}
	}
}

void BlobTracker::processPureData()
{

}

void BlobTracker::setBlobAlgorithm(blobDetection::BlobDetection *bd)
{
	this->bd = bd;
}

void BlobTracker::doUpEvent(TouchData td)
{
	for (std::map<std::string, ITouchListener *>::iterator it = listenersMap.begin(); it!=listenersMap.end(); ++it)
	{
		(*it).second->fingerUp(td);
	}
}

void BlobTracker::doDownEvent(TouchData td)
{
	for (std::map<std::string, ITouchListener *>::iterator it = listenersMap.begin(); it!=listenersMap.end(); ++it)
	{
		(*it).second->fingerDown(td);
	}
}

void BlobTracker::doUpdateEvent(TouchData td)
{
	for (std::map<std::string, ITouchListener *>::iterator it = listenersMap.begin(); it!=listenersMap.end(); ++it)
	{
		(*it).second->fingerUpdate(td);
	}
}
